#include "collisionStrategy.h"

#include <cmath>
#include <vector>
#include <algorithm>

#include "ioManager.h"

using std::cout; using std::endl; using std::vector;

void RectangularCollisionStrategy::draw() const
{
	IOManager::getInstance().printMessageAt("Rectangular Collision Strategy", 320, 10);
}

bool RectangularCollisionStrategy::execute(Sprite& obj1, Sprite& obj2) const
{
	float left1 = obj1.X();
	float left2 = obj2.X();
	float right1 = left1+obj1.getFrame()->getWidth();
	float right2 = left2+obj2.getFrame()->getWidth();
	float top1 = obj1.Y();
	float top2 = obj2.Y();
	float bottom1 = top1+obj1.getFrame()->getHeight();
	float bottom2 = top2+obj2.getFrame()->getHeight();
	if ( right1 < left2 ) return false;
	if ( left1 > right2 ) return false;
	if ( bottom1 < top2 ) return false;
	if ( bottom2 < top1 ) return false;
	return true;
}

void AABBCollisionStrategy::draw() const
{
	IOManager::getInstance().printMessageAt("Rectangular Collision Strategy", 320, 10);
}

bool AABBCollisionStrategy::execute(Sprite& obj1, Sprite& obj2) const
{
	// Calculate AABB vectors
	Vector2f p1 = obj1.getCenter();
	Vector2f xw1((float)obj1.getFrame()->getWidth()/2.0,0);
	Vector2f yw1(0,(float)obj1.getFrame()->getHeight()/2.0);

	Vector2f p2 = obj2.getCenter();
	Vector2f xw2((float)obj2.getFrame()->getWidth()/2.0,0);
	Vector2f yw2(0,(float)obj2.getFrame()->getHeight()/2.0);

	Vector2f diff = p1-p2;

	// Check for overlap on x-axis (if the x-axis is a seperating axis)
	if(abs(diff[0]) > xw1[0] + xw2[0]) {
		return false;
	}
	float overlap_x = (xw1[0] + xw2[0]) - abs(diff[0]);

	// Check for overlap on y-axis (if the y-axis is a seperating axis)
	if(abs(diff[1]) > yw1[1] + yw2[1]) {
		return false;
	}
	float overlap_y = (yw1[1] + yw2[1]) - abs(diff[1]);

	Vector2f mtd;

	// Compute maximum translation distance (mtd) vector
	if(overlap_x < overlap_y) {
		mtd = Vector2f(1,0) * overlap_x * (diff[0] > 0 ? 1 : -1);
	} else {
		mtd = Vector2f(0,1) * overlap_y * (diff[1] > 0 ? 1 : -1);
	}

	// Apply mtd vector to position (obj1 is gaurunteed to be dynamic)
	obj1.setPosition(obj1.getPosition() + mtd);
	if(obj2.getType() == TYPE_DYNAMIC)
		obj2.setPosition(obj2.getPosition() - mtd);

	// Update components of velocity
	Vector2f u1 = obj1.getVelocity();
	Vector2f u2 = obj2.getVelocity();
	Vector2f v1, v2;
	float m1 = obj1.getMass();
	float m2 = obj2.getMass();

	// Both objects are dynamic, apply conservation of momentum
	if(obj2.getType() == TYPE_DYNAMIC) {
		v1 = ((m1-m2)/(m1+m2))*u1 + ((2*m2)/(m1+m2)) * u2;
		v2 = ((m2-m1)/(m1+m2))*u2 + ((2*m1)/(m1+m2)) * u1;
		if(overlap_x < overlap_y) {
			v1[0] = v1[0] * obj1.getRestitution();
			v1[1] = v1[1] * 0.9;
			v2[0] = v2[0] * obj2.getRestitution();
			v2[1] = v2[1] * 0.9;
		} else {
			v1[0] = v1[0] * obj1.getFriction();
			v1[1] = v1[1] * obj1.getRestitution();
			v2[0] = v2[0] * obj2.getFriction();
			v2[1] = v2[1] * obj2.getRestitution();
		}
		obj1.setVelocity(v1);
		obj2.setVelocity(v2);
	// obj2 is static and treate like it has infinite mass
	} else {
		if(overlap_x < overlap_y) {
			v1[0] = -u1[0] * obj1.getRestitution();
			v1[1] = u1[1] * 1.0;
		} else {
			v1[0] = u1[0] * obj1.getFriction();
			v1[1] = -u1[1] * obj1.getRestitution();
		}
		obj1.setVelocity(v1);
	}

	return true;
}


float MidPointCollisionStrategy::distance(float x1, float y1, float x2, float y2) const
{
	float x = x1-x2, y = y1-y2;
	return hypot(x, y);
}

void MidPointCollisionStrategy::draw() const
{
	IOManager::getInstance().printMessageAt("Distance from Middle Strategy", 320, 10);
}

bool MidPointCollisionStrategy::execute(Sprite& obj1, Sprite& obj2) const
{
	int COLLISION_DISTANCE = obj1.getFrame()->getWidth()/2 + obj2.getFrame()->getWidth()/2;
	// std::cout << "DISTANCE: " << COLLISION_DISTANCE << std::endl;
	float xobj1 = obj1.X()+obj1.getFrame()->getWidth()/2;
	float yobj1 = obj1.Y()+obj1.getFrame()->getHeight()/2;
	float xobj2 = obj2.X() + obj2.getFrame()->getWidth()/2;
	float yobj2 = obj2.Y() + obj2.getFrame()->getHeight()/2;
	// float d = distance(xobj1, yobj1, xobj2, yobj2);
	// cout << "distance: " << d << endl;
	return distance(xobj1, yobj1, xobj2, yobj2) < COLLISION_DISTANCE;
}

void PerPixelCollisionStrategy::draw() const
{
	IOManager::getInstance().printMessageAt("Per-Pixel Collision Strategy", 320, 10);
}

bool PerPixelCollisionStrategy::execute(Sprite& obj1, Sprite& obj2) const
{
	RectangularCollisionStrategy strategy;
	if ( not strategy.execute(obj1, obj2) ) return false;
	// If we got this far, we know that the sprite rectangles intersect!

	Vector2f p1 = obj1.getPosition();
	Vector2f p2 = obj2.getPosition();
	const Frame * const frame1 = obj1.getFrame();
	const Frame * const frame2 = obj2.getFrame();

	int o1Left = p1[0]; 
	int o1Right = o1Left+frame1->getWidth();

	int o2Left = p2[0]; 
	int o2Right = o2Left+frame2->getWidth();
	vector<int> x;
	x.reserve(4);
	x.push_back( o1Left );
	x.push_back( o1Right );
	x.push_back( o2Left );
	x.push_back( o2Right );
	std::sort( x.begin(), x.end() );

	int o1Up = p1[1];
	int o1Down = o1Up+frame1->getHeight();
	int o2Up = p2[1];
	int o2Down = o2Up+frame2->getHeight();
	vector<int> y;
	y.reserve(4);
	y.push_back( o1Up );
	y.push_back( o1Down );
	y.push_back( o2Up );
	y.push_back( o2Down );
	std::sort( y.begin(), y.end() );

	// Some debug statements:
	// cout << "(left, right): " << x[1] << ", " << x[2] << endl;
	// cout << "(top, bottom): " << y[1] << ", " << y[2] << endl;

	SDL_Surface* surface1 = frame1->getSurface();
	SDL_Surface* surface2 = frame2->getSurface();

	SDL_LockSurface(surface1);
	SDL_LockSurface(surface2);
	unsigned pixels1;
	unsigned pixels2;
	for (int i = x[1]; i < x[2]; ++i) {
		for (int j = y[1]; j < y[2]; ++j) {
			// check pixels in obj1 and obj2!
			pixels1 = obj1.getPixel(i, j);
			pixels2 = obj2.getPixel(i, j);
			if ( pixels1 != 16711935 && pixels2 != 16711935) {
				SDL_UnlockSurface(surface1);
				SDL_UnlockSurface(surface2);
				return true;
			}
		}
	}
	SDL_UnlockSurface(surface1);
	SDL_UnlockSurface(surface2);

	return false;
}

